#!/usr/bin/python

import roslib
roslib.load_manifest('srs_ui_pro')
import time
import rospy

import grasping_functions
from srs_ui_pro.srv import *
import openravepy

class simulation_c():

	def __init__(self):
		print "-------------------------------------------------------------------------";
		rospy.loginfo("/grasp_simulator service is ready.");
		self.env = grasping_functions.init_simulator();
		print "-------------------------------------------------------------------------";


	def simulation(self, server_goal):
		x = time.time();
		rospy.loginfo("/simulation service has been called...");

		grasping_functions.grasp_view(self.env, server_goal.object_id, server_goal.grasp, server_goal.object_pose);

		print "Time employed: " + str(time.time() - x);
		print "---------------------------------------";
		return ;

	def simulation_server(self):
		s = rospy.Service('/grasp_simulator', simulation, self.simulation);


if __name__ == '__main__':
	rospy.init_node('simulation')
	SCRIPT = simulation_c()
	SCRIPT.simulation_server();
	rospy.spin()
